
#include "gh.h"

#define EPSILON 0.001f

typedef struct {
    union {
        struct { float x, y, z, w; };
        float f[4];
    };
} SWvec;

typedef struct {
    union {
        SWvec r[4];
        struct {
            float _00, _01, _02, _03;
            float _10, _11, _12, _13;
            float _20, _21, _22, _23;
            float _30, _31, _32, _33;
        };
        float m[4][4];
    };
} SWmat;

GH_INLINE SWvec *swVecSet(SWvec *dst, float x, float y, float z, float w) {
    dst->x = x; dst->y = y; dst->z = z; dst->w = w;
    return dst;
}

GH_INLINE SWmat *swMatSet(SWmat *dst,
        float _00, float _01, float _02, float _03,
        float _10, float _11, float _12, float _13,
        float _20, float _21, float _22, float _23,
        float _30, float _31, float _32, float _33) {
    dst->_00 = _00; dst->_01 = _01; dst->_02 = _02; dst->_03 = _03;
    dst->_10 = _10; dst->_11 = _11; dst->_12 = _12; dst->_13 = _13;
    dst->_20 = _20; dst->_21 = _21; dst->_22 = _22; dst->_23 = _23;
    dst->_30 = _30; dst->_31 = _31; dst->_32 = _32; dst->_33 = _33;
    return dst;
}

GH_INLINE SWvec *swVecSub(SWvec *dst, const SWvec *v0, const SWvec *v1) {
    dst->x = v0->x - v1->x; dst->y = v0->y - v1->y; dst->z = v0->z - v1->z; dst->w = v0->w - v1->w;
    return dst;
}

GH_INLINE SWmat *swMatRotationQuat(SWmat *dst, const SWvec *q) {
    float yy2 = 2 * q->y * q->y;
    float xy2 = 2 * q->x * q->y;
    float xz2 = 2 * q->x * q->z;
    float yz2 = 2 * q->y * q->z;
    float zz2 = 2 * q->z * q->z;
    float wz2 = 2 * q->w * q->z;
    float wy2 = 2 * q->w * q->y;
    float wx2 = 2 * q->w * q->x;
    float xx2 = 2 * q->x * q->x;
    return swMatSet(dst,
        1 - yy2 - zz2,     xy2 + wz2,     xz2 - wy2, 0,
            xy2 - wz2, 1 - xx2 - zz2,     yz2 + wx2, 0,
            xz2 + wy2,     yz2 - wx2, 1 - xx2 - yy2, 0,
                    0,             0,             0, 1);
}

GH_INLINE SWvec *swQuatMul(SWvec *dst, const SWvec *q0, const SWvec *q1) {
    SWvec t;
    t.x = q0->y * q1->z - q0->z * q1->y + q1->w * q0->x + q0->w * q1->x;
    t.y = q0->z * q1->x - q0->x * q1->z + q1->w * q0->y + q0->w * q1->y;
    t.z = q0->x * q1->y - q0->y * q1->x + q1->w * q0->z + q0->w * q1->z;
    t.w = q0->w * q1->w - q0->x * q1->x - q0->y * q1->y - q0->z * q1->z;
    *dst = t;
    return dst;
}

GH_INLINE float swVec2Dot(const SWvec *v0, const SWvec *v1) {
    return v0->x * v1->x + v0->y * v1->y;
}
GH_INLINE float swVec3Dot(const SWvec *v0, const SWvec *v1) {
    return v0->x * v1->x + v0->y * v1->y + v0->z * v1->z;
}
GH_INLINE float swVec4Dot(const SWvec *v0, const SWvec *v1) {
    return v0->x * v1->x + v0->y * v1->y + v0->z * v1->z + v0->w * v1->w;
}

GH_INLINE SWvec *swVec3Cross(SWvec *dst, const SWvec *v0, const SWvec *v1) {
    float x = v0->y * v1->z - v1->y * v0->z;
    float y = v0->z * v1->x - v1->z * v0->x;
    float z = v0->x * v1->y - v1->x * v0->y;
    dst->x = x; dst->y = y; dst->z = z;
    return dst;
}

GH_INLINE float swVec2Length(const SWvec *v) {
    return sqrtf(swVec2Dot(v, v));
}
GH_INLINE float swVec3Length(const SWvec *v) {
    return sqrtf(swVec3Dot(v, v));
}
GH_INLINE float swVec4Length(const SWvec *v) {
    return sqrtf(swVec4Dot(v, v));
}

GH_INLINE SWvec *swVec2NormalizeF(SWvec *dst, const SWvec *v) {
    float s = 1.f / swVec2Length(v);
    dst->x *= s; dst->y *= s; dst->z *= s; dst->w *= s;
    return dst;
}
GH_INLINE SWvec *swVec3NormalizeF(SWvec *dst, const SWvec *v) {
    float s = 1.f / swVec3Length(v);
    dst->x *= s; dst->y *= s; dst->z *= s; dst->w *= s;
    return dst;
}
GH_INLINE SWvec *swVec4NormalizeF(SWvec *dst, const SWvec *v) {
    float s = 1.f / swVec4Length(v);
    dst->x *= s; dst->y *= s; dst->z *= s; dst->w *= s;
    return dst;
}

GH_INLINE SWmat *swMatIdentity(SWmat *dst) {
    return swMatSet(dst,
        1.f, 0.f, 0.f, 0.f,
        0.f, 1.f, 0.f, 0.f,
        0.f, 0.f, 1.f, 0.f,
        0.f, 0.f, 0.f, 1.f);
}
GH_INLINE SWmat *swMatTranslation(SWmat *dst, float x, float y, float z) {
    return swMatSet(dst,
        1.f, 0.f, 0.f, 0.f,
        0.f, 1.f, 0.f, 0.f,
        0.f, 0.f, 1.f, 0.f,
          x,   y,   z, 1.f);
}
GH_INLINE SWmat *swMatScaling(SWmat *dst, float x, float y, float z) {
    return swMatSet(dst,
          x, 0.f, 0.f, 0.f,
        0.f,   y, 0.f, 0.f,
        0.f, 0.f,   z, 0.f,
        0.f, 0.f, 0.f, 1.f);
}

GH_INLINE SWmat *swMatRotationX(SWmat *dst, float rad) {
    float sinv = sinf(rad);
    float cosv = cosf(rad);
    return swMatSet(dst,
        1.f,   0.f,  0.f, 0.f,
        0.f,  cosv, sinv, 0.f,
        0.f, -sinv, cosv, 0.f,
        0.f,   0.f,  0.f, 1.f);
}
GH_INLINE SWmat *swMatRotationY(SWmat *dst, float rad) {
    float sinv = sinf(rad);
    float cosv = cosf(rad);
    return swMatSet(dst,
         cosv, 0.f, -sinv, 0.f,
          0.f, 1.f,   0.f, 0.f,
         sinv,   0,  cosv, 0.f,
          0.f, 0.f,   0.f, 1.f);
}
GH_INLINE SWmat *swMatRotationZ(SWmat *dst, float rad) {
    float sinv = sinf(rad);
    float cosv = cosf(rad);
    return swMatSet(dst,
         cosv, sinv, 0.f, 0.f,
        -sinv, cosv, 0.f, 0.f,
          0.f,  0.f, 1.f, 0.f,
          0.f,  0.f, 0.f, 1.f);
}

GH_INLINE SWmat *swMatLookAt(SWmat *dst,
        float ex, float ey, float ez,
        float ax, float ay, float az,
        float ux, float uy, float uz) {

    SWvec eyePt, atPt, upVec, axisX, axisY, axisZ;
    swVecSet(&eyePt, ex, ey, ez, 1.f);
    swVecSet(&atPt, ax, ay, az, 1.f);
    swVecSet(&upVec, ux, uy, uz, 0.f);

    swVecSub(&axisZ, &eyePt, &atPt);
    swVec3NormalizeF(&axisZ, &axisZ);
    swVec3Cross(&axisX, &upVec, &axisZ);
    swVec3Cross(&axisY, &axisZ, &axisX);

    return swMatSet(dst,
        axisX.x, axisY.x, axisZ.x, 0.f,
        axisX.y, axisY.y, axisZ.y, 0.f,
        axisX.z, axisY.z, axisZ.z, 0.f,
        -swVec3Dot(&axisX, &eyePt), -swVec3Dot(&axisY, &eyePt), -swVec3Dot(&axisZ, &eyePt), 1.f);
}

GH_INLINE SWmat *swMatPerspective(SWmat *dst, float fovy, float aspect, float n, float f) {
    float hfovy = fovy * .5f;
    float sy = cosf(hfovy) / sinf(hfovy);
    float sx = sy / aspect;
    return swMatSet(dst,
         sx, 0.f,            0.f,  0.f,
        0.f,  sy,            0.f,  0.f,
        0.f, 0.f,   -(f+n)/(f-n), -1.f,
        0.f, 0.f, -2.f*f*n/(f-n),  0.f);
}

GH_INLINE SWvec *swVec4Transform(SWvec *dst, const SWvec *v, const SWmat *m) {
    SWvec t;
    t.x = v->x * m->r[0].x + v->y * m->r[1].x + v->z * m->r[2].x + v->w * m->r[3].x;
    t.y = v->x * m->r[0].y + v->y * m->r[1].y + v->z * m->r[2].y + v->w * m->r[3].y;
    t.z = v->x * m->r[0].z + v->y * m->r[1].z + v->z * m->r[2].z + v->w * m->r[3].z;
    t.w = v->x * m->r[0].w + v->y * m->r[1].w + v->z * m->r[2].w + v->w * m->r[3].w;
    *dst = t;
    return dst;
}

GH_INLINE SWmat *swMatMul(SWmat *dst, const SWmat *m0, const SWmat *m1) {
    SWmat res;
    float x, y, z, w;

    x = m0->m[0][0];
    y = m0->m[0][1];
    z = m0->m[0][2];
    w = m0->m[0][3];
    res.m[0][0] = (m1->m[0][0]*x)+(m1->m[1][0]*y)+(m1->m[2][0]*z)+(m1->m[3][0]*w);
    res.m[0][1] = (m1->m[0][1]*x)+(m1->m[1][1]*y)+(m1->m[2][1]*z)+(m1->m[3][1]*w);
    res.m[0][2] = (m1->m[0][2]*x)+(m1->m[1][2]*y)+(m1->m[2][2]*z)+(m1->m[3][2]*w);
    res.m[0][3] = (m1->m[0][3]*x)+(m1->m[1][3]*y)+(m1->m[2][3]*z)+(m1->m[3][3]*w);

    x = m0->m[1][0];
    y = m0->m[1][1];
    z = m0->m[1][2];
    w = m0->m[1][3];
    res.m[1][0] = (m1->m[0][0]*x)+(m1->m[1][0]*y)+(m1->m[2][0]*z)+(m1->m[3][0]*w);
    res.m[1][1] = (m1->m[0][1]*x)+(m1->m[1][1]*y)+(m1->m[2][1]*z)+(m1->m[3][1]*w);
    res.m[1][2] = (m1->m[0][2]*x)+(m1->m[1][2]*y)+(m1->m[2][2]*z)+(m1->m[3][2]*w);
    res.m[1][3] = (m1->m[0][3]*x)+(m1->m[1][3]*y)+(m1->m[2][3]*z)+(m1->m[3][3]*w);

    x = m0->m[2][0];
    y = m0->m[2][1];
    z = m0->m[2][2];
    w = m0->m[2][3];
    res.m[2][0] = (m1->m[0][0]*x)+(m1->m[1][0]*y)+(m1->m[2][0]*z)+(m1->m[3][0]*w);
    res.m[2][1] = (m1->m[0][1]*x)+(m1->m[1][1]*y)+(m1->m[2][1]*z)+(m1->m[3][1]*w);
    res.m[2][2] = (m1->m[0][2]*x)+(m1->m[1][2]*y)+(m1->m[2][2]*z)+(m1->m[3][2]*w);
    res.m[2][3] = (m1->m[0][3]*x)+(m1->m[1][3]*y)+(m1->m[2][3]*z)+(m1->m[3][3]*w);

    x = m0->m[3][0];
    y = m0->m[3][1];
    z = m0->m[3][2];
    w = m0->m[3][3];
    res.m[3][0] = (m1->m[0][0]*x)+(m1->m[1][0]*y)+(m1->m[2][0]*z)+(m1->m[3][0]*w);
    res.m[3][1] = (m1->m[0][1]*x)+(m1->m[1][1]*y)+(m1->m[2][1]*z)+(m1->m[3][1]*w);
    res.m[3][2] = (m1->m[0][2]*x)+(m1->m[1][2]*y)+(m1->m[2][2]*z)+(m1->m[3][2]*w);
    res.m[3][3] = (m1->m[0][3]*x)+(m1->m[1][3]*y)+(m1->m[2][3]*z)+(m1->m[3][3]*w);

    *dst = res;
    return dst;
}

static void SWvecPrint(const char *name, const SWvec *v) {
    printf("%s: %f %f %f %f\n", name, v->x, v->y, v->z, v->w);
}
static void GHvecPrint(const char *name, GHvec v) {
    printf("%s: %f %f %f %f\n", name, ghVecGetX(v), ghVecGetY(v), ghVecGetZ(v), ghVecGetW(v));
}

GH_INLINE int ScalarEqual(float s0, float s1) {
    return fabsf(s0 - s1) <= EPSILON;
}

static int VecEqual(GHvec v, const SWvec *vref) {
    return
        ScalarEqual(ghVecGetX(v), vref->x) &&
        ScalarEqual(ghVecGetY(v), vref->y) &&
        ScalarEqual(ghVecGetZ(v), vref->z) &&
        ScalarEqual(ghVecGetW(v), vref->w);
}

static int MatEqual(const GHmat *m, const SWmat *mref) {
    return
        VecEqual(m->r[0], &mref->r[0]) &&
        VecEqual(m->r[1], &mref->r[1]) &&
        VecEqual(m->r[2], &mref->r[2]) &&
        VecEqual(m->r[3], &mref->r[3]);
}

static int Test_MatRotationQuat(void) {
    GHmat m;
    SWmat msw;
    GHvec q;
    SWvec qsw;

    q = ghVecSet(1.0f, 2.0f, 3.0f, 4.0f);
    m = ghMatRotationQuat(q);
    swVecSet(&qsw, 1.0f, 2.0f, 3.0f, 4.0f);
    swMatRotationQuat(&msw, &qsw);
    if (!MatEqual(&m, &msw)) {
        return -1;
    }

    q = ghVecSet(10.0f, 2.4f, -3.0f, 4.2f);
    m = ghMatRotationQuat(q);
    swVecSet(&qsw, 10.0f, 2.4f, -3.0f, 4.2f);
    swMatRotationQuat(&msw, &qsw);
    if (!MatEqual(&m, &msw)) {
        return -1;
    }

    return 0;
}

static int Test_QuatMul(void) {
    GHvec q0, q1;
    SWvec sq0, sq1;
    int i;

    q0 = ghVecSet(.1f, .2f, .3f, .4f);
    q1 = ghVecSet(.4f, .2f, -.3f, .25f);
    swVecSet(&sq0, .1f, .2f, .3f, .4f);
    swVecSet(&sq1, .4f, .2f, -.3f, .25f);

    for (i = 0; i < 10; ++i) {
        q0 = ghQuatMul(q0, q1);
        swQuatMul(&sq0, &sq0, &sq1);
    }
    if (!VecEqual(q0, &sq0)) {
        return -1;
    }

    return 0;
}

static int Test_Vec2Dot(void) {
    GHvec v0, v1;
    SWvec sv0, sv1;
    float sr;

    v0 = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v1 = ghVecSet(-2.f, .2f, -13.f, 4.f);
    v0 = ghVec2Dot(v0, v1);

    swVecSet(&sv0, 1.f, 2.f, 3.f, 4.f);
    swVecSet(&sv1, -2.f, .2f, -13.f, 4.f);
    sr = swVec2Dot(&sv0, &sv1);

    if (!ScalarEqual(ghVecGetX(v0), sr) ||
        !ScalarEqual(ghVecGetY(v0), sr) ||
        !ScalarEqual(ghVecGetZ(v0), sr) ||
        !ScalarEqual(ghVecGetW(v0), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec3Dot(void) {
    GHvec v0, v1;
    SWvec sv0, sv1;
    float sr;

    v0 = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v1 = ghVecSet(-2.f, .2f, -13.f, 4.f);
    v0 = ghVec3Dot(v0, v1);

    swVecSet(&sv0, 1.f, 2.f, 3.f, 4.f);
    swVecSet(&sv1, -2.f, .2f, -13.f, 4.f);
    sr = swVec3Dot(&sv0, &sv1);

    if (!ScalarEqual(ghVecGetX(v0), sr) ||
        !ScalarEqual(ghVecGetY(v0), sr) ||
        !ScalarEqual(ghVecGetZ(v0), sr) ||
        !ScalarEqual(ghVecGetW(v0), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec4Dot(void) {
    GHvec v0, v1;
    SWvec sv0, sv1;
    float sr;

    v0 = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v1 = ghVecSet(-2.f, .2f, -13.f, 4.f);
    v0 = ghVec4Dot(v0, v1);

    swVecSet(&sv0, 1.f, 2.f, 3.f, 4.f);
    swVecSet(&sv1, -2.f, .2f, -13.f, 4.f);
    sr = swVec4Dot(&sv0, &sv1);

    if (!ScalarEqual(ghVecGetX(v0), sr) ||
        !ScalarEqual(ghVecGetY(v0), sr) ||
        !ScalarEqual(ghVecGetZ(v0), sr) ||
        !ScalarEqual(ghVecGetW(v0), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec3Cross(void) {
    GHvec v0, v1;
    SWvec sv0, sv1;

    v0 = ghVecSet(10.f, 112.f, -3.f, 0.f);
    v1 = ghVecSet(-2.2f, .2f, -13.22f, 0.f);
    v0 = ghVec3Cross(v0, v1);

    swVecSet(&sv0, 10.f, 112.f, -3.f, 0.f);
    swVecSet(&sv1, -2.2f, .2f, -13.22f, 0.f);
    swVec3Cross(&sv0, &sv0, &sv1);

    if (!VecEqual(v0, &sv0)) {
        return -1;
    }

    return 0;
}

static int Test_Vec2Length(void) {
    GHvec v;
    SWvec sv;
    float sr;

    v = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v = ghVec2Length(v);

    swVecSet(&sv, 1.f, 2.f, 3.f, 4.f);
    sr = swVec2Length(&sv);

    if (!ScalarEqual(ghVecGetX(v), sr) ||
        !ScalarEqual(ghVecGetY(v), sr) ||
        !ScalarEqual(ghVecGetZ(v), sr) ||
        !ScalarEqual(ghVecGetW(v), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec3Length(void) {
    GHvec v;
    SWvec sv;
    float sr;

    v = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v = ghVec3Length(v);

    swVecSet(&sv, 1.f, 2.f, 3.f, 4.f);
    sr = swVec3Length(&sv);

    if (!ScalarEqual(ghVecGetX(v), sr) ||
        !ScalarEqual(ghVecGetY(v), sr) ||
        !ScalarEqual(ghVecGetZ(v), sr) ||
        !ScalarEqual(ghVecGetW(v), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec4Length(void) {
    GHvec v;
    SWvec sv;
    float sr;

    v = ghVecSet(1.f, 2.f, 3.f, 4.f);
    v = ghVec4Length(v);

    swVecSet(&sv, 1.f, 2.f, 3.f, 4.f);
    sr = swVec4Length(&sv);

    if (!ScalarEqual(ghVecGetX(v), sr) ||
        !ScalarEqual(ghVecGetY(v), sr) ||
        !ScalarEqual(ghVecGetZ(v), sr) ||
        !ScalarEqual(ghVecGetW(v), sr)) {
        return -1;
    }

    return 0;
}

static int Test_Vec2NormalizeF(void) {
    GHvec v;
    SWvec sv;

    v = ghVecSet(10.f, 112.f, -3.f, .123f);
    v = ghVec2NormalizeF(v);

    swVecSet(&sv, 10.f, 112.f, -3.f, .123f);
    swVec2NormalizeF(&sv, &sv);

    if (!VecEqual(v, &sv)) {
        return -1;
    }

    return 0;
}

static int Test_Vec3NormalizeF(void) {
    GHvec v;
    SWvec sv;

    v = ghVecSet(10.f, 112.f, -3.f, .123f);
    v = ghVec3NormalizeF(v);

    swVecSet(&sv, 10.f, 112.f, -3.f, .123f);
    swVec3NormalizeF(&sv, &sv);

    if (!VecEqual(v, &sv)) {
        return -1;
    }

    return 0;
}

static int Test_Vec4NormalizeF(void) {
    GHvec v;
    SWvec sv;

    v = ghVecSet(10.f, 112.f, -3.f, .123f);
    v = ghVec4NormalizeF(v);

    swVecSet(&sv, 10.f, 112.f, -3.f, .123f);
    swVec4NormalizeF(&sv, &sv);

    if (!VecEqual(v, &sv)) {
        return -1;
    }

    return 0;
}

static int Test_MatTranslation(void) {
    GHmat m;
    SWmat sm;

    m = ghMatTranslation(1.f, 2.f, 3.f);
    swMatTranslation(&sm, 1.f, 2.f, 3.f);
    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatScaling(void) {
    GHmat m;
    SWmat sm;

    m = ghMatScaling(1.f, 2.f, 3.f);
    swMatScaling(&sm, 1.f, 2.f, 3.f);
    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatRotationX(void) {
    GHmat m;
    SWmat sm;

    m = ghMatRotationX(.57f);
    swMatRotationX(&sm, .57f);
    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatRotationY(void) {
    GHmat m;
    SWmat sm;

    m = ghMatRotationY(1.57f);
    swMatRotationY(&sm, 1.57f);
    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatRotationZ(void) {
    GHmat m;
    SWmat sm;

    m = ghMatRotationZ(-.57f);
    swMatRotationZ(&sm, -.57f);
    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatLookAt(void) {
    GHmat m;
    SWmat sm;
    GHvec e, a, u;

    e = ghVecSet(1.f, 2.f, 3.f, 4.f);
    a = ghVecSet(0.f, 0.f, 0.f, 1.f);
    u = ghVecSet(0.f, 1.f, 0.f, 0.f);
    m = ghMatLookAt(e, a, u);

    swMatLookAt(&sm, 1.f, 2.f, 3.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f);

    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_MatPerspective(void) {
    GHmat m;
    SWmat sm;

    m = ghMatPerspective(GH_PIDIV4, 1.333f, 1.f, 100.f);
    swMatPerspective(&sm, GH_PIDIV4, 1.333f, 1.f, 100.f);

    if (!MatEqual(&m, &sm)) {
        return -1;
    }

    return 0;
}

static int Test_Vec4Transform(void) {
    GHmat m;
    GHvec v;
    SWmat sm;
    SWvec sv;

    m = ghMatPerspective(GH_PIDIV4, 1.333f, 1.f, 100.f);
    swMatPerspective(&sm, GH_PIDIV4, 1.333f, 1.f, 100.f);

    v = ghVecSet(1.f, 2.f, 3.f, 4.f);
    swVecSet(&sv, 1.f, 2.f, 3.f, 4.f);

    v = ghVec4Transform(v, &m);
    swVec4Transform(&sv, &sv, &sm);

    if (!VecEqual(v, &sv)) {
        return -1;
    }

    return 0;
}

static int Test_MatMul(void) {
    GHmat m0, m1;
    SWmat sm0, sm1;

    m0 = ghMatIdentity();
    m1 = ghMatTranslation(10.f, 20.f, 30.f);

    swMatIdentity(&sm0);
    swMatTranslation(&sm1, 10.f, 20.f, 30.f);

    m0 = ghMatMul(&m0, &m1);
    swMatMul(&sm0, &sm0, &sm1);

    if (!MatEqual(&m0, &sm0)) {
        return -1;
    }
    if (!ScalarEqual(ghVecGetX(m0.r[3]), 10.f) ||
        !ScalarEqual(ghVecGetY(m0.r[3]), 20.f) ||
        !ScalarEqual(ghVecGetZ(m0.r[3]), 30.f) ||
        !ScalarEqual(ghVecGetW(m0.r[3]), 1.f)) {
        return -1;
    }

    return 0;
}
/*
GHvec Profile(void) {
    uint64_t t;
    GHvec v, v0 = { 1, 2, 3, 4 };
    int i;
    double t0, t1;

    //v = 1.0f;
    v = ghVecSet(1.5f, 2.0f, 3.0f, -4.0f);
    v0 = ghVecSet(4.0f, 2.5f, 3.0f, 4.0f);
    v = ghQuatMul(v, v0);
    t0 = ghTimeGetMicroSec();
    t = ghTimeRdtsc();

    for (i = 0; i < 100000; ++i) {
        v = ghQuatMul(v, v0);
    }

    t = ghTimeRdtsc() - t;
    t1 = ghTimeGetMicroSec();
    printf("Cycles: %u\ttime: %f\n", (unsigned int) t, t1 - t0);
    printf("V = (%f, %f, %f, %f)\n", ghVecGetX(v), ghVecGetY(v), ghVecGetZ(v), ghVecGetW(v));
    return v;
}
*/

static int RunTest(const char *name, int (*testFunc)(void)) {
    int r;
    printf("Running test %-24s ", name);
    r = testFunc();
    printf("%s\n", r < 0 ? "FAILED" : "PASSED");
    return r;
}

int ghAppBegin(void) {
    GHvec v = ghVecReplicate(-2.2f);
    v = ghVecFloor(v);
    printf("\nXXXXXXXXXXXXXX %f\n", ghVecGetX(v));

    RunTest("ghMatRotationQuat", Test_MatRotationQuat);
    RunTest("ghQuatMul", Test_QuatMul);
    RunTest("ghVec2Dot", Test_Vec2Dot);
    RunTest("ghVec3Dot", Test_Vec3Dot);
    RunTest("ghVec4Dot", Test_Vec4Dot);
    RunTest("ghVec3Cross", Test_Vec3Cross);
    RunTest("ghVec2Length", Test_Vec2Length);
    RunTest("ghVec3Length", Test_Vec3Length);
    RunTest("ghVec4Length", Test_Vec4Length);
    RunTest("ghVec2NormalizeF", Test_Vec2NormalizeF);
    RunTest("ghVec3NormalizeF", Test_Vec3NormalizeF);
    RunTest("ghVec4NormalizeF", Test_Vec4NormalizeF);
    RunTest("ghMatTranslation", Test_MatTranslation);
    RunTest("ghMatScaling", Test_MatScaling);
    RunTest("ghMatRotationX", Test_MatRotationX);
    RunTest("ghMatRotationY", Test_MatRotationY);
    RunTest("ghMatRotationZ", Test_MatRotationZ);
    RunTest("ghMatLookAt", Test_MatLookAt);
    RunTest("ghMatPerspective", Test_MatPerspective);
    RunTest("ghVec4Transform", Test_Vec4Transform);
    RunTest("ghMatMul", Test_MatMul);

    

    return 0;
}

int ghAppFrame(double dt, double time) {
    return 1;
}

int ghAppEnd(void) {
    return 0;
}

int ghAppMouseCallback(int x, int y, int action) {
    return 0;
}
